#include "bits/stdc++.h"
#include "ros/ros.h"
#include "unistd.h"
#include <sensor_msgs/LaserScan.h>

using namespace std;
#define BIZHANG 6
#define MINDIST 0.8

#define div 2

int   state;   // flag: 0=find obstacle ;1=move around obstacle
int   flag = 0;
float findMinDistance(vector<float> ranges)
{
    float mindistance = ranges[175];
    for ( int i = 220; i < 230; i++ ) {
        if ( ranges[i] < mindistance ) {
            mindistance = ranges[i];
        }
    }
    return mindistance;
}

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    ros::param::get("state", state);

    if ( findMinDistance(msg->ranges) > MINDIST && state != BIZHANG ) {
        return;
    }
    else if ( findMinDistance(msg->ranges) <= MINDIST && state != BIZHANG ) {
        int state_last = state;
        ros::param::set("state", BIZHANG);

        cout << "\033[44mfind obstacle\033[0m" << endl;
        ros::param::set("gear", 1);
        for ( int i = 0; i < 200 / div; i++ ) {
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
//        ros::param::set("gear", 2);
//        for ( int i = 0; i < 150 / div; i++ ) {   // 斜300
//            ros::param::set("state", BIZHANG);
//            usleep(10000);
//        }

        ros::param::set("gear", 5);
        for ( int i = 0; i < 450 / div; i++ ) {   // 斜700
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
        ros::param::set("gear", 2);
        for ( int i = 0; i < 200 / div; i++ ) {   // 斜75
            ros::param::set("state", BIZHANG);
            usleep(10000);
        }
        cout << "\033[44mavoid over\033[0m" << endl;
        ros::param::set("state", state_last);
    }
    flag = 1;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "avoid");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 1, laserCallback);
    ros::Rate       looprate(100);
    int             light = 0;
    while(light == 0)
    {
        ros::param::get("light", light);
        usleep(10000);
    }
    sleep(10);
    while ( ros::ok() ) {
        if ( flag == 0 )
            ros::spinOnce();
        else
            break;
        looprate.sleep();
    }
    return 0;
}
